/*
8-10-2011 Updated to use floating point math. NEEDS ALOT OF WORK!!@#!$
8-13-2011 fixed a bunch of math and replaced a long string of code with atan2
*/
#include <NewSoftSerial.h>
#include "TinyGPS.h"
#include <LiquidCrystal.h>
#include <Servo.h>

Servo servo1; Servo servo2; 
LiquidCrystal lcd(12, 11, 5, 4, 3, 2);
NewSoftSerial nss(7, 8, true); // my gps data is inverted!!
TinyGPS gps;


void setup()
{
  Serial.begin(4800);
  nss.begin(4800);
  // set up the LCD's number of columns and rows:
  lcd.begin(16, 2);
}

void loop()
{
  float flat,flon;
  float start_lat = 37.379600, start_lon = -94.698593, start_alt = 374.32; // this is test info where the plane is

  int b;
  
  while (nss.available())
  {
    int c = nss.read();
    
    if ( c > 0 )
    {
      b = c;
    }
    
    if (gps.encode(b))
    {
      //gps.stats(&chars, &sentences, &failed_checksum);
      //gps.get_position(&lat, &lon); // LONG NOT FLOATING POINT
      servo1.attach(9);
      servo2.attach(10);
      gps.f_get_position(&flat, &flon);
      
      float falt = gps.f_altitude();
      double dist = gps.distance_between (start_lat, start_lon, flat, flon);
      double az = get_az(start_lat, start_lon, flat, flon);
      double el = get_el(start_alt, falt, dist);
      
      Serial.print("Lat : ");
      Serial.print(flat,6);
      Serial.print(" Lon : ");
      Serial.print(flon,6);
      Serial.println();
      Serial.print("Az : ");
      Serial.print(az,1);
      Serial.print(" El : ");
      Serial.print(el,1); 
      Serial.print(" Distance : ");
      Serial.print(dist,1);
      Serial.print(" meters");
      Serial.println();
      // output to lcd screen
      lcd.setCursor(0, 0);
      lcd.print(flat,4);
      lcd.print(" ");
      lcd.print(flon,4);
      lcd.setCursor(0, 1);
      lcd.print(az,0);
      lcd.print(" ");
      lcd.print(el,0);
      lcd.print(" ");
      lcd.print(dist,0);
      // write servo
      servo1.write((int)az);
      servo2.write((int)el);
      delay(50);
    } 
    
    // Logic doesnt work for the above if for some reason..
    /*else
    {
      Serial.print("No GPS Fix");
      Serial.println();
      lcd.setCursor(0, 0);
      lcd.print("No GPS Fix");
      delay(1000);
    }*/
}
}

// all of this should be moved to a header file after testing Tyler 8/9/2011
// Seems to work fixed a radian - degree issue
double get_az(double lat1, double lon1, double lat2, double lon2)
{
  // calculated the azmuth between two points on earth using floating point math.
  // may need some optmization
  // tyler 2011-08-09
  const double pi = 3.14159265; // pi you gotz to love it
  const double r2d = 57.2957795; // radians to degrees this number = 180/pi
  double dlat = lat2 - lat1;
  double dlon = lon2 - lon1;
  double y = sin(lon2-lon1)*cos(lat2);
  double x = cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(lon2-lon1);
  double tc1 = 0;
  tc1 = atan2(y,x);

  if (tc1 < 0)
    return tc1 + 360;
  else
    return tc1;
}

// needs testing but it seems to work so far 8-13-11 tyler
double get_el(double plane, double ground, double dist)
{
  // elevation and distance in meters
  // requires elevation be based on the same point i.e sea level or a home position
  //
  // raduis of the earth in meters 6378100
  // diamater of the earth in meters 12756200
  
  return 57.2957795 * (asin((plane-ground) / sqrt(pow((plane - ground),2) + (pow(dist,2)))) - dist/12756200);
}